import java.util.ArrayList;

/**
 * @author Remi Destigny
 * Helper static class for laser sensors, store the information about the sensor and provide 
 * an utility function to find the intersection point between each laser and the obstacles
 */
public final class LaserHelper {
	
	public static double angleIncrement = -1;
	public static double startAngle = -1;
	public static double endAngle = -1;
	
	/**
	 * Compute the point where laser and obstacle intersect for each laser echoe
	 * @param distanceArray Echoe array containing the distance between the robot and the obstacle for each laser
	 * @return An array of points where obstacles are found
	 */
	public static ArrayList<Vector> getLaserStopPoints(ArrayList<Double> distanceArray) {
		double angle = startAngle;
		ArrayList<Vector> rotatedPointArray = new ArrayList<Vector>();
		// Each laser is "angleIncrement" away from the next one starting at "startAngle"
		for (Double d : distanceArray) {
			// Apply a rotation to the point ( An echoe point is a point at (echoe distance,0) from the robot with a given rotation )
			double newX = d * Math.cos(angle) + Math.sin(angle);
			double newY = -1 * d * Math.sin(angle) + Math.cos(angle);
			rotatedPointArray.add(new Vector(newX, newY));
			angle += angleIncrement;
		}
		return rotatedPointArray;
	}

}
